package edu.wpi.first.wpilibj.swarm;
import edu.wpi.first.wpilibj.Joystick;

class Teleop {
	Joystick                joy1;
	Joystick                joy2;
	Joystick                joy3;
	RobotMain               robomain;
	Balancer                balancer;
	boolean                 balancing;
	DSInputs                io;
	
	public Teleop(RobotMain robotmain) {
		robomain   = robotmain;
		joy1       = new Joystick(1);
		joy2       = new Joystick(2);
		joy3       = new Joystick(3);
		balancer   = new Balancer(robomain.drivetrain);
		balancing  = false;
		io         = new DSInputs();
	}
	
	public void init() {
		io.init();
	}
	
	private double limit (double min, double val, double max) {
		if (val < min) val = min;
		if (val > max) val = max;
		return val;
	}
	
	private double nullzone(double in, double nullzone) {
		return (in - limit(-nullzone, in, nullzone))/(1.0-nullzone);
	}
	
	public void periodic() {
		if (io.getReset()) robomain.drivetrain.resetGyro();
		
		double x = (joy1.getX() + joy2.getX())/2 + joy3.getX();
		double y = -(joy1.getY() + joy2.getY())/2 - joy3.getY();
		double twist = (-joy1.getY() + joy2.getY())/2 + joy3.getTwist();
		x     = nullzone(x, 0.1);
		y     = nullzone(y, 0.1);
		twist = nullzone(twist, 0.1);
		
		if (!io.getBalance()) {
			balancing = false;
			if (!io.getHighSpeed()) {
				x     *= 0.3;
				y     *= 0.3;
				twist *= 0.3;
			}
			robomain.drivetrain.setInput(x, y, twist, io.getField());
		} else {
			if (!balancing) balancer.initbalance();
			balancing = true;
			balancer.balance(x, y, twist, io.getField());
		}
		if (io.getCompressor() && !robomain.compressor_enabled) {
			robomain.compressor.start();
			robomain.compressor_enabled = true;
		} else if (!io.getCompressor() && robomain.compressor_enabled) {
			robomain.compressor.stop();
			robomain.compressor_enabled = false;
		}
		
		robomain.arm.setEnabled(!io.getCompressor());
		robomain.kicker.setEnabled(!io.getCompressor());
		if (io.getArmUp()) {
			robomain.arm.setDisabledOut(-1.0);
		} else {
			robomain.arm.setDisabledOut(0.0);
		}
		
		if (io.getArmUp()) {
			robomain.arm.setTarget(0.0);
		} else if (io.getArmReady()) {
			robomain.arm.setTarget(0.61);
		} else if (io.getArmLiftReady()) {
			robomain.arm.setTarget(0.73);
		} else if (io.getArmDown()) {
			robomain.arm.setTarget(1.0);
		}
		
		if (io.getBrushForward()) {
			robomain.kicker.brushOn();
		} else if (io.getBrushReverse()) {
			robomain.kicker.brushRev();
		} else {
			robomain.kicker.brushOff();
		}
		
		robomain.kicker.setKicking(io.getKick());
		
		robomain.kicker.setPos(io.getDistance());
	}
}